from enum import Enum
import math

class SpcCar:
    __moto1 = 0
    __moto2 = 0
    __moto3 = 0
    __moto4 = 0

    __powerX = 0.0
    __powerY = 0.0

    __speed = 0.0
    __yaw = 0.0

    def __init__(self):
        print("SpcCar create")

    def __del__(self):
        print("SpcCar delete")

    def setPowerXY(self,powerX,powerY,speed = None, yaw = None):
        self.__powerX = powerX
        self.__powerY = powerY

        if speed == None:
            self.__speed = math.sqrt(powerX**2 + powerY**2)
        else:
            self.__speed = speed

        if yaw == None:
            if powerX > 0 and powerY > 0:  # 1
                self.__yaw = math.atan(powerY / powerX) / math.pi * 180.0
            elif powerX > 0 and powerY < 0:   # 4
                self.__yaw = math.atan(powerY / powerX) / math.pi * 180.0
            elif powerX < 0 and powerY > 0 :   # 2
                self.__yaw = 180.0 - math.atan(powerY / -powerX) / math.pi * 180.0
            elif powerX < 0 and powerY < 0:
                self.__yaw = math.atan(powerY / powerX) / math.pi * 180.0 - 180.0
            elif powerX == 0 and powerY > 0:
                self.__yaw = 90.0
            elif powerX == 0 and powerY < 0:
                self.__yaw = -90.0
            elif powerX > 0 and powerY == 0:
                self.__yaw = 0.0
            elif powerX < 0 and powerY == 0:
                self.__yaw = 180.0
            else:
                self.__yaw = 0.0
        else:
            self.__yaw = yaw

        m1x = powerX
        m2x = -powerX
        m3x = -powerX
        m4x = powerX

        m1y = powerY
        m2y = powerY
        m3y = -powerY
        m4y = -powerY

        self.__moto1 = m1x + m1y
        self.__moto2 = m2x + m2y
        self.__moto3 = m3x + m3y
        self.__moto4 = m4x + m4y
        return

    def setSpeedYaw(self, speed, yaw):
        rad = yaw / 180.0 * math.pi
        mx = speed * math.cos(rad)
        my = speed * math.sin(rad)
        self.setPowerXY(mx,my, speed, yaw)
        return